classdef ARR_Kinematic_Model < handle
    properties (Access = public)
        P_f = Point();
        P_m = Point();
        P_r = Point();
        P_f_c1 = Point();
        P_f_c2 = Point();
        P_f_c3 = Point();
        P_f_c4 = Point();
        P_r_c1 = Point();
        P_r_c2 = Point();
        P_r_c3 = Point();
        P_r_c4 = Point();
        L_f = 1.846;
        L_r = 1.843;
%         L_f = 5;
%         L_r = 5;
        L_1 = 2;
        L_2 = 3;
        steer_wheel_angle = 0;
        steer_wheel_angle_rate = 0;    
    end
    
    methods (Access = public)
        function obj = ARR_Kinematic_Model(x_f,y_f,yaw_f,ga_angle,mode)
            obj.P_f.x = x_f;
            obj.P_f.y = y_f;
            obj.P_f.velocity = mode*0.000000001;
            obj.P_f.yaw = yaw_f;
            obj.P_f.yaw_rate = 0.0;
            obj.P_m.x = obj.P_f.x - obj.L_f * cos(obj.P_f.yaw);
            obj.P_m.y = obj.P_f.y - obj.L_f * sin(obj.P_f.yaw);
            obj.steer_wheel_angle = ga_angle;
            obj.steer_wheel_angle_rate = 0.0;
            obj.P_r.yaw = yaw_f + ga_angle;
            obj.P_r.yaw_rate = 0.0;
            obj.P_r.x = obj.P_m.x - obj.L_r * cos(obj.P_r.yaw);
            obj.P_r.y = obj.P_m.y - obj.L_r * sin(obj.P_r.yaw);
            obj.P_r.velocity = mode*0.000000001;
            
            obj.P_f_c1.x = obj.P_f.x + (0.5*obj.L_1*cos(obj.P_f.yaw) - 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c1.y = obj.P_f.y + (0.5*obj.L_1*sin(obj.P_f.yaw) + 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c2.x = obj.P_f.x + (0.5*obj.L_1*cos(obj.P_f.yaw) + 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c2.y = obj.P_f.y + (0.5*obj.L_1*sin(obj.P_f.yaw) - 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c3.x = obj.P_f.x + (-0.5*obj.L_1*cos(obj.P_f.yaw) + 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c3.y = obj.P_f.y + (-0.5*obj.L_1*sin(obj.P_f.yaw) - 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c4.x = obj.P_f.x + (-0.5*obj.L_1*cos(obj.P_f.yaw) - 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c4.y = obj.P_f.y + (-0.5*obj.L_1*sin(obj.P_f.yaw) + 0.5*obj.L_2*cos(obj.P_f.yaw));
            
            obj.P_r_c1.x = obj.P_r.x + (0.5*obj.L_1*cos(obj.P_r.yaw) - 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c1.y = obj.P_r.y + (0.5*obj.L_1*sin(obj.P_r.yaw) + 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c2.x = obj.P_r.x + (0.5*obj.L_1*cos(obj.P_r.yaw) + 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c2.y = obj.P_r.y + (0.5*obj.L_1*sin(obj.P_r.yaw) - 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c3.x = obj.P_r.x + (-0.5*obj.L_1*cos(obj.P_r.yaw) + 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c3.y = obj.P_r.y + (-0.5*obj.L_1*sin(obj.P_r.yaw) - 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c4.x = obj.P_r.x + (-0.5*obj.L_1*cos(obj.P_r.yaw) - 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c4.y = obj.P_r.y + (-0.5*obj.L_1*sin(obj.P_r.yaw) + 0.5*obj.L_2*cos(obj.P_r.yaw));
        end
        
        function State_Update(obj, vf, delta_t, ga_angle_rate)
            if obj.steer_wheel_angle >= pi/6 && ga_angle_rate > 0
                ga_angle_rate = 0;
            elseif obj.steer_wheel_angle <= -pi/6 && ga_angle_rate < 0
                ga_angle_rate = 0;
            end
            if ga_angle_rate > pi/3
                ga_angle_rate = pi/3;
            elseif ga_angle_rate < -pi/3
                ga_angle_rate = -pi/3;
            end
%             obj.P_r.x = obj.P_r.x + vr * cos(obj.P_r.yaw) * delta_t;
%             obj.P_r.y = obj.P_r.y + vr * sin(obj.P_r.yaw) * delta_t;
%             obj.P_r.yaw_rate = ((vr * sin(obj.steer_wheel_angle)) - (obj.L_f * cos(obj.steer_wheel_angle) * ga_angle_rate))/(-obj.L_r-(obj.L_f * cos(obj.steer_wheel_angle)));
%             obj.P_r.yaw = obj.P_r.yaw + delta_t * obj.P_r.yaw_rate;
%             obj.P_m.x = obj.P_r.x + obj.L_r * cos(obj.P_r.yaw);
%             obj.P_m.y = obj.P_r.y + obj.L_r * sin(obj.P_r.yaw);
%             obj.steer_wheel_angle = obj.steer_wheel_angle + (ga_angle_rate * delta_t);
%             obj.steer_wheel_angle_rate = ga_angle_rate;
            obj.P_f.velocity = vf;
            obj.P_f.x = obj.P_f.x + vf * cos(obj.P_f.yaw) * delta_t;
            obj.P_f.y = obj.P_f.y + vf * sin(obj.P_f.yaw) * delta_t;
            obj.P_f.yaw_rate = ((vf * sin(obj.steer_wheel_angle)) + (obj.L_r * ga_angle_rate))/(-obj.L_r-(obj.L_f * cos(obj.steer_wheel_angle)));
            obj.P_f.yaw = obj.P_f.yaw + delta_t * obj.P_f.yaw_rate;
            obj.P_r.velocity = (vf*cos(obj.steer_wheel_angle))-(obj.L_f*obj.P_f.yaw_rate*sin(obj.steer_wheel_angle));
            obj.P_m.x = obj.P_f.x - obj.L_f * cos(obj.P_f.yaw);
            obj.P_m.y = obj.P_f.y - obj.L_f * sin(obj.P_f.yaw);
            obj.steer_wheel_angle = obj.steer_wheel_angle + (ga_angle_rate * delta_t);
            obj.steer_wheel_angle_rate = ga_angle_rate;
            if obj.steer_wheel_angle > pi/6
                obj.steer_wheel_angle = pi/6;
            elseif obj.steer_wheel_angle < -pi/6
                obj.steer_wheel_angle = -pi/6;
            end
%             obj.P_f.yaw = obj.P_r.yaw - obj.steer_wheel_angle;
%             obj.P_f.yaw_rate = obj.P_r.yaw_rate - obj.steer_wheel_angle_rate;
%             obj.P_f.x = obj.P_m.x + obj.L_f * cos(obj.P_f.yaw);
%             obj.P_f.y = obj.P_m.y + obj.L_f * sin(obj.P_f.yaw);            

            obj.P_r.yaw = obj.P_f.yaw + obj.steer_wheel_angle;
            obj.P_r.yaw_rate = obj.P_f.yaw_rate + obj.steer_wheel_angle_rate;
            obj.P_r.x = obj.P_m.x - obj.L_r * cos(obj.P_r.yaw);
            obj.P_r.y = obj.P_m.y - obj.L_r * sin(obj.P_r.yaw);            

            
            obj.P_f_c1.x = obj.P_f.x + (0.5*obj.L_1*cos(obj.P_f.yaw) - 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c1.y = obj.P_f.y + (0.5*obj.L_1*sin(obj.P_f.yaw) + 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c2.x = obj.P_f.x + (0.5*obj.L_1*cos(obj.P_f.yaw) + 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c2.y = obj.P_f.y + (0.5*obj.L_1*sin(obj.P_f.yaw) - 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c3.x = obj.P_f.x + (-0.5*obj.L_1*cos(obj.P_f.yaw) + 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c3.y = obj.P_f.y + (-0.5*obj.L_1*sin(obj.P_f.yaw) - 0.5*obj.L_2*cos(obj.P_f.yaw));
            obj.P_f_c4.x = obj.P_f.x + (-0.5*obj.L_1*cos(obj.P_f.yaw) - 0.5*obj.L_2*sin(obj.P_f.yaw));
            obj.P_f_c4.y = obj.P_f.y + (-0.5*obj.L_1*sin(obj.P_f.yaw) + 0.5*obj.L_2*cos(obj.P_f.yaw));
            
            obj.P_r_c1.x = obj.P_r.x + (0.5*obj.L_1*cos(obj.P_r.yaw) - 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c1.y = obj.P_r.y + (0.5*obj.L_1*sin(obj.P_r.yaw) + 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c2.x = obj.P_r.x + (0.5*obj.L_1*cos(obj.P_r.yaw) + 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c2.y = obj.P_r.y + (0.5*obj.L_1*sin(obj.P_r.yaw) - 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c3.x = obj.P_r.x + (-0.5*obj.L_1*cos(obj.P_r.yaw) + 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c3.y = obj.P_r.y + (-0.5*obj.L_1*sin(obj.P_r.yaw) - 0.5*obj.L_2*cos(obj.P_r.yaw));
            obj.P_r_c4.x = obj.P_r.x + (-0.5*obj.L_1*cos(obj.P_r.yaw) - 0.5*obj.L_2*sin(obj.P_r.yaw));
            obj.P_r_c4.y = obj.P_r.y + (-0.5*obj.L_1*sin(obj.P_r.yaw) + 0.5*obj.L_2*cos(obj.P_r.yaw));
            
            fprintf('ARR vf %f vr %f \n',obj.P_f.velocity,obj.P_r.velocity);
        end
    end
end
